#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>

#include <asio_driver/device/buzzer_src.hpp>
#include <asio_driver/device/rc_sbus.hpp>
#include <asio_driver/utils/range.hpp>
#include <asio_driver_special/imu_wit_cmp10a.hpp>
#include <asio_driver_special/motor_brt_can.hpp>
#include <asio_driver_special/motor_dc_pwm.hpp>

#include <rclcpp/utilities.hpp>
#include <tf2/tf2/LinearMath/Quaternion.h>

#include <numbers>
#include <sensor_msgs/msg/imu.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/float64.hpp>

class Devices
{
  private:
    bool isOpened = false;

  public:
    AD::AsioContext::SharePtr context;
    AD::Special::MotorBrtCan motorLeft;
    AD::Special::MotorBrtCan motorRight;
    AD::Special::MotorBrtCan motorExternalLeft;
    AD::Special::MotorBrtCan motorExternalRight;
    AD::Special::IMUWitCmp10a imu;
    AD::Buzzer buzzer;

  public:
    Devices(AD::AsioContext::SharePtr context)
        : context(context), motorLeft(context), motorRight(context),
          motorExternalLeft(context), motorExternalRight(context), imu(context),
          buzzer(context)
    {
    }
    ~Devices()
    {
        if (isOpened)
        {
            Close();
        }
    }

    void Open()
    {
        std::unique_ptr<AD::Can> can = std::make_unique<AD::Can>(context);
        AD::CanBus::SharePtr can_bus = AD::CanBus::Create(context);
        motorLeft.Open(can_bus, AD::Special::MotorBrtCan::MotorConfig{
                                    .address = 3, .poleCount = 14});
        motorRight.Open(can_bus, AD::Special::MotorBrtCan::MotorConfig{
                                     .address = 4, .poleCount = 14});
        can->Open("can0");
        can_bus->Open(std::move(can));
        imu.Open("/dev/ttyS6", 115200);
        buzzer.Open(34, true);
        isOpened = true;
    }

    void Close()
    {
        motorLeft.Close();
        motorRight.Close();
        motorExternalLeft.Close();
        motorExternalRight.Close();
        imu.Close();
        buzzer.Close();
        isOpened = false;
    }
};

struct State
{
    double gyrox;
    double gyroy;
    double gyroz;
    double accx;
    double accy;
    double accz;
    double angley;
    double anglex;
    double anglez;

    double speed;

    double leftCurrent;
    double rightCurrent;

    double leftRPM;
    double rightRPM;
};

class ChassisServer : public rclcpp::Node
{
  private:
    using MotorTorque = std_msgs::msg::Float64;
    rclcpp::Subscription<MotorTorque>::SharedPtr leftMotorSub, rightMotorSub;
    using WheelSpeed = std_msgs::msg::Float64;
    rclcpp::Publisher<WheelSpeed>::SharedPtr leftSpeedPub, rightSpeedPub;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imuPub;
    rclcpp::TimerBase::SharedPtr timer;

    std::unique_ptr<Devices> devices;
    AD::AsioContext::SharePtr context;
    State state;
    double radius = 0.14;
    double scale = 100;
    double speedLimit = 0.5;

    double initAngleZ = 0;
    bool first = true;

  public:
    ChassisServer() : rclcpp::Node("chassis_server")
    {
        declare_parameter<double>("force_scale", 100.0);
        scale = get_parameter("force_scale").as_double();
        declare_parameter<double>("speed_limit", 100.0);
        speedLimit = get_parameter("speed_limit").as_double();
        RCLCPP_INFO(get_logger(), "init chassis devices");
        context = AD::AsioContext::CreatAsioContext();
        devices = std::make_unique<Devices>(context);
        devices->Open();
        RCLCPP_INFO(get_logger(), "chassis devices inited");

        leftMotorSub = create_subscription<MotorTorque>(
            "left_torque", 1, [this](const MotorTorque::SharedPtr msg)
            { state.leftCurrent = msg->data; });
        rightMotorSub = create_subscription<MotorTorque>(
            "right_torque", 1, [this](const MotorTorque::SharedPtr msg)
            { state.rightCurrent = msg->data; });

        leftSpeedPub = create_publisher<WheelSpeed>("left_speed", 1);
        rightSpeedPub = create_publisher<WheelSpeed>("right_speed", 1);
        imuPub = create_publisher<sensor_msgs::msg::Imu>("imu", 1);

        timer = create_wall_timer(std::chrono::milliseconds(10),
                                  std::bind(&ChassisServer::step, this));
    }

    void step()
    {
        context->SpinOnce();

        auto imuData = devices->imu.Read();

        state.gyrox = AD::Utils::InRange(imuData.gyro[1], -30, 30)
                          ? -imuData.gyro[1]
                          : state.gyrox;
        state.gyroy = AD::Utils::InRange(imuData.gyro[0], -30, 30)
                          ? -imuData.gyro[0]
                          : state.gyroy;
        state.gyroz = AD::Utils::InRange(imuData.gyro[2], -30, 30)
                          ? imuData.gyro[2]
                          : state.gyroz;
        state.accx = AD::Utils::InRange(imuData.acc[1], -100000, 100000)
                         ? imuData.acc[1]
                         : state.accx;
        state.accy = AD::Utils::InRange(imuData.acc[0], -100000, 100000)
                         ? imuData.acc[0]
                         : state.accy;
        state.accz = AD::Utils::InRange(imuData.acc[2], -100000, 100000)
                         ? imuData.acc[2]
                         : state.accz;
        state.anglex = AD::Utils::InRange(imuData.angle[1], -30, 30)
                           ? -imuData.angle[1]
                           : state.anglex;
        state.angley = AD::Utils::InRange(imuData.angle[0], -30, 30)
                           ? -imuData.angle[0]
                           : state.angley;
        // for angel z, its range in [-180,180]
        state.anglez = imuData.angle[2];
        if (first)
        {
            initAngleZ = state.anglez;
            first = false;
        }
        state.anglez = state.anglez - initAngleZ;
        if (state.anglez > 180)
            state.anglez = state.anglez - 360;
        else if (state.anglez <= -180)
            state.anglez = state.anglez + 360;
        

        const double factor = 1.0 / 60.0 * 2 * std::numbers::pi * radius;
        double left = devices->motorLeft.ReadState().rpm * factor;
        double right = devices->motorRight.ReadState().rpm * factor;
        WheelSpeed SpeedMsg;
        SpeedMsg.data = left;
        leftSpeedPub->publish(SpeedMsg);
        SpeedMsg.data = right;
        rightSpeedPub->publish(SpeedMsg);

        if (std::abs(left) > speedLimit || std::abs(right) > speedLimit)
        {
            devices->motorLeft.SetCurrent(0);
            devices->motorRight.SetCurrent(0);
            RCLCPP_ERROR(get_logger(), "speed overlap");
            rclcpp::shutdown();
        }
        else
        {
            devices->motorLeft.SetCurrent(state.leftCurrent * scale);
            devices->motorRight.SetCurrent(state.rightCurrent * scale);
        }
        devices->motorLeft.SendHeartBeat();
        devices->motorRight.SendHeartBeat();

        sensor_msgs::msg::Imu imuMsg;
        tf2::Quaternion q;
        q.setRPY(state.anglex * std::numbers::pi / 180.0,
                 state.angley * std::numbers::pi / 180.0,
                 state.anglez * std::numbers::pi / 180.0);
        imuMsg.orientation.x = q.x();
        imuMsg.orientation.y = q.y();
        imuMsg.orientation.z = q.z();
        imuMsg.orientation.w = q.w();
        imuMsg.angular_velocity.x = state.gyrox * std::numbers::pi / 180.0;
        imuMsg.angular_velocity.y = state.gyroy * std::numbers::pi / 180.0;
        imuMsg.angular_velocity.z = state.gyroz * std::numbers::pi / 180.0;
        imuMsg.linear_acceleration.x = state.accx;
        imuMsg.linear_acceleration.y = state.accy;
        imuMsg.linear_acceleration.z = state.accz;
        imuMsg.header.stamp = get_clock()->now();
        imuMsg.header.frame_id = "footprint";
        imuPub->publish(imuMsg);
    }
};
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ChassisServer>());
    rclcpp::shutdown();
}